perm filename SSERVO.FAI[VV,BGB]1 blob
sn#105002 filedate 1974-11-21 generic text, type C, neo UTF8
COMMENT ā VALID 00011 PAGES
C REC PAGE DESCRIPTION
C00001 00001 VALID 00020 PAGES
C00003 00002 entry sservo,scalstp
C00005 00003 BEGINNING OF SW SERVO PROGRAM TO SIERRA TV CAMERA
C00007 00004 loop through pots calculate the distance to go
C00011 00005 servo in progress, read petit clock and calculate delta t
C00013 00006 check if new velocity is to be calculated
C00015 00007 calculate elapsed time and distance since last velocity update
C00017 00008 check if destination at hand
C00019 00009 set up iris motor (not velocity servoed)
C00021 00010 end of servo - check for hung motors
C00023 00011 constants and buffers
C00029 ENDMK
Cā;
entry sservo,scalstp
title sservo routine
internal state,p4,p5,p6,p7,p8,l4,l5,l6,l7,l8,sref,pot
;registers available to user
state: 0 ;status bits
p4: 0 ;latest pot readings(pan)
p5: 0 ;tilt
p6: 0 ;focus
p7: 0 ;zoom
p8: 0 ;iris
sref: 0 ;reference voltage
l4: 0 ;final pot values(pan)
l5: 0 ;tilt
l6: 0 ;focus
l7: 0 ;zoom
l8: 0 ;iris
; BEGINNING OF SW SERVO PROGRAM TO SIERRA TV CAMERA
SSERVO: CONSZ 40
JRST WRONG ;dismiss if spacewar started on pdp-10
movei 1,=30
movem 1,delt
;read the pots and set up byte pointers
CONI 204,7
ANDI 7,7 ;a/d busy?
SKIPN 7
JRST COUNT
SKIPE POT ;IGNORE IF SERVOING, WE SHOULD HAVE INITED IT
JRST BUSY
COUNT: MOVEI 6,10 ;DATA MISSED COUNT
TURNON: CONO 204,4250 ;set up hsdc
CONO 424,722030 ;auto index from ch. 72
MOVE 5,[-2,,0]
GETIT: MOVEI 4,200
CONSO 204,1000
SOJGE 4,.-1 ; WAIT FOR DONE FLAG
JUMPL 4,HUNG
CONSZ 204,10000
JRST [ CONO 424,4000 ; DATA MISSED, TRY AGAIN
SOJG 6,[MOVEI 3,12
SOJGE 3,$.
JRST TURNON]
JRST DMISS]
DATAI 204,buffer(5)
AOBJN 5,GETIT ;go back for second word
cono 424,4000 ;turn off a/d
MOVE 5,[POINT 12,buffer]; save reference voltage
ildb 10,5
lsh 10,30
ash 10,-30
fsc 10,233
movem 10,sref
skipe pot
jrst lop-4 ; JUMP AROUND SERVO CODE IF ONLY READING POTS
move 2,[point 7,drvwrd] ;set up byte pointers
MOVE 15,[-5,,0]
; loop through pots calculate the distance to go
POINT: ILDB 10,5 ;INCREMENT POINTER AND LOAD BYTE FROM BUFFER
move 11,l4(15) ;load destination pot value
fmpr 11,sref
fix 11,233000
LSH 10,30 ;sign shift buffer
ASH 10,-30 ;reshift right
movem 10,hold ;store buffer in ac 13
SUB 10,11 ;subtract destination from buffer
camn 15,[-4,,1]
movem 10,tilt ;save tilt value for gravity loading
move 14,10 ;save the signed value(togo)
movem 10,diff
movms 10
movem 10,togo ;distance left to go
camn 15,[-1,,4]
jrst iris ; iris control is not velocity servo
skipe inital
jrst inprog ; servo in progress, update
move 13,hold ; first time initialization for each pot
movem 13,begin(15)
movem 10,prev(15)
fsc 10,233
fdvr 10,[207454,,0] ;calculate no. of segments
fmpr 10,[207410,,0] ;number of msecs required(66 msecs/segment)
fadr 10,[212764,,0] ;add in base time(1000 msecs)
movem 10,traj ;total time required
move 10,14
fsc 10,233
fdvr 10,traj ;average velocity required(signed)
movem 10,veloc(15)
setzm 10
move 11,14
movms 11
caig 11,6 ;check if (absolute)distance required less
jrst zero ; than tolerance
setzm sign(15)
movei 10,20 ;turn on motors
skipg 14
jrst .+3
movei 10,160
setom sign(15)
zero: movem 10,speed(15)
coni 730,oldtim ;store start time
jrst stptst ; check stop conditions
; servo in progress, read petit clock and calculate delta t
inprog: coni 730,time
move 4,time ;set up time comparisons
move 11,oldtim
lsh 4,-24 ;shift out microsecond field
lsh 11,-24
setzm 13
camn 4,11
jrst timeok ;skip if any change in secs and minutes
move 4,time ;calculate total elapsed time
lsh 4,-32
move 11,oldtim
lsh 11,-32
came 4,11 ;compare minute counts
add 13,[344703400] ;add in minute count in microsecs
move 7,time
lsh 7,12
lsh 7,-36
move 11,oldtim
lsh 11,12
lsh 11,-36
sub 7,11
imul 7,[3641100]
add 13,7
timeok: move 4,time
move 11,oldtim
lsh 4,=16
lsh 4,-=16
lsh 11,=16
lsh 11,-=16
sub 4,11 ;total elapsed time
add 13,4
fsc 13,233 ;float time
move 4,13
fdvr 4,[212764,,0] ;change to msecs
move 3,4 ; check timeout loop and velocity update
fix 3,233000 ;test if spacewar module has run too long
movem 3,totel ;total elapased time in msecs
caig 3,=9000 ;dismiss after 9 secs
jrst timchk
move 7,[1,,0]
iorm 7,state ;non-termination of sservo
jrst trnoff
; check if new velocity is to be calculated
timchk: move 14,totel
sub 14,del1
caig 14,=30 ;elapsed time > 30 msecs
jrst check
fsc 14,233 ;floating elapsed time since last vel update
movem 14,delt
move 14,totel
movem 14,del1
setom velcal
setom velcal+1
setom velcal+2
setom velcal+3
check: skipn velcal(15) ;calculate distance covered if velocity update
jrst ok ; flag set
move 6,begin(15) ;calculate distance covered
move 10,hold
sub 6,10
fsc 6,233 ;distance covered
movem 6,distnz(15)
move 14,hold
movem 14,begin(15)
ok: move 6,togo ;calculate if distance to target < 300
cail 6,=320
jrst mumble
skipe final(15)
jrst decrem
setom final(15)
move 7,diff
movem 7,stop(15)
move 7,veloc(15)
fdvr 7,[203400,,0]
movem 7,velstr(15)
move 7,speed(15)
move 7,index
decrem: move 6,velstr(15) ; decrement velocity if final turned on
setzm 13
move 4,togo
aos 13
subi 4,=80
skiple 4
jrst .-3
fsc 13,233
fmpr 6,13
skipg diff ;check for sign difference
jrst less
skipg stop(15)
jrst less+1
jrst pos
; calculate elapsed time and distance since last velocity update
less: skipl stop(15)
fmpr 6,[576400,,0]
pos: movem 6,veloc(15)
mumble: skipn velcal(15)
jrst next
move 11,distnz(15)
skipg index
move 10,begin(15)
camn 10,hold
jrst addtim
sub 10,hold
fsc 10,233
fadr 11,10 ;additional distance covered
addtim: skipg index
move 7,delt ;time interval
move 10,totel
camn 10,del1
jrst doit
fsc 10,233
move 13,del1
fsc 13,233
fsbr 10,13
fadr 7,10 ;add in additional time
doit: skipg index
; do feedback calculations
fdvr 11,7 ;average velocity over past 20 msecs
skipg index
fsbr 11,veloc(15) ;difference average velocity and planned
; velocity(pot units/tic)
fdvr 11,veloc(15) ;percentage difference(signed)
fmpr 11,gain(15) ;multiply by feedback gain
fix 11,233000 ;fix velocity gain
move 10,speed(15)
trnn 10,100 ;add in velocity gain
jrst ahead
skipg diff
movns 11
sub 10,11
caig 10,100
movei 10,100
cail 10,177
subi 10,177
jrst set
;check if destination at hand
ahead: skipl diff
movns 11
add 10,11
cail 10,77
movei 10,77
caig 10,
addi 10,177
set: cain 10,
movei 10,1
caig 15,2 ;gravity loading for tilt and focus
cain 15,
jrst nograv
move 7,[0,,3777]
sub 7,tilt
fsc 7,233
fdvr 7,[215400,,0] ;divide by 4096
move 6,7
fmpr 6,6
fmpr 6,7
fmpr 7,[201622,,045536]
fmpr 6,[577266,,527571]
fadr 6,7 ;calculate sin(x)
fmpr 6,grvtrm(15)
fix 6,23300
skipl diff
jrst downer
add 10,6
cail 10,77
movei 10,77
jrst nograv
downer: add 10,6
cail 10,177
movei 10,177
nograv: movem 10,speed(15)
stptst: move 7,togo ; test stop conditions
caml 7,dedbnd(15)
jrst next
setzm speed(15)
setom beend(15)
next: move 1,speed(15) ;set up drive word for this motor
idpb 1,2 ;load proper voltage into drive word
move 13,hold
movem 13,last(15)
setzm velcal(15)
jrst loop
; set up iris motor (not velocity servoed)
iris: cail 10,20 ;tolerance on iris - special case
jrst .+3
setom beend(15)
jrst loop
SKIPE INITAL
JRST [ MOVEI 11,(10)
SUB 11,PREV(15)
MOVMS 11
CAILE 11,3
JRST .+1
JRST LOOP] ;iris is hung
MOVEM 10,PREV(15)
move 11,drvwrd
tro 11,100 ;set enable bit (bit 29)
skipl hold ;determine proper direction
tro 11,200 ;set close bit(bit 28)
movem 11,drvwrd
loop: aobjn 15,point ;branch back for next function
setom inital
datao 410,drvwrd ; end of servo if motors all off
skipn drvwrd
jrst trnoff
setzm drvwrd
jrst count ; return to read pots again
; end of servo - check for hung motors
trnoff: setzm 1
skipn beend
iori 1,4000
skipn beend+1
iori 1,2000
skipn beend+2
iori 1,1000
skipn beend+3
iori 1,10000
skipn beend+4
iori 1,100000
iorm 1,state
move 13,[null,,null+1] ; enter here if only reading pots
blt 13,final+4
setzm 15
move 13,[point 12,buffer,11]
lop: ildb 14,13 ;deposit final pot readings in P4-P8
lsh 14,30
ash 14,-30
fsc 14,233
fdvr 14,sref
movem 14,P4(15)
aos 15
caie 15,5
jrst lop
movei 1,1
iorm 1,state
setzm pot ; if read pots only, set flags and dismiss
KILL: DATAO 410,[0] ; force all motors off
setzm inital
CALLI 400024 ; and exit
;assorted error routines
busy: movei 7,100
jrst dmiss+1
hung: cono 424,4000
skipa 7,[200]
dmiss: movei 7,40
iorm 7,state
JRST TRNOFF
wrong: movei 7,400
jrst dmiss+1
; constants and buffers
last: block 5 ;last pot reading
veloc: block 5 ;average velocity needed
speed: block 5 ;current speed
begin: block 5 ;initial pot reading
stop: BLOCK 5
velstr: block 5
prev: block 5 ;initial distance to go
velcal: block 5
distnz: block 5
sign: block 5 ;direction of motor movement
gain: 576246,,314632 ;-1.35
576363,,146315 ;-1.05
576400,,0 ;-1.0
576363,,146315 ;-1.05
576400,,0 ;-1.0(not used)
dedbnd: 6 ;drift distance
=24
6
6
6 ;(not used)
grvtrm: 0 ;not used
203500,,0 ;gravity loading term for tilt(5.0)
576400,,0 ;focus(1.0)
0 ;not used
0 ;not used
S4: BLOCK 5
index: 0
totel: 0
;various registers and stuff
tilt: 0
delt: 0
time: 0
buffer: block 2 ;raw pot readings
oldtim: 0 ;last time read
null: 0 ;this starts the null out region
drvwrd: 0
hold: 0
togo: 0
traj: 0
diff: 0
first: 0
del1: 0
pot: 0
inital: 0
beend: block 5
final: block 5
scalstp:spcwar kill
popj 17,
end